﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

using Microsoft.Xna.Framework;

namespace XNALibrary
{
    public class MatrixHelper
    {
        public static Matrix CreateMatrixRow(Vector3 x, Vector3 y, Vector3 z)
        {
            Matrix m = Matrix.Identity;

            m.M11 = x.X; m.M12 = x.Y; m.M13 = x.Z;
            m.M21 = y.X; m.M22 = y.Y; m.M23 = y.Z;
            m.M31 = z.X; m.M32 = z.Y; m.M33 = z.Z;

            return m;
        }

        public static Matrix CreateMatrixCol(Vector3 x, Vector3 y, Vector3 z)
        {
            Matrix m = Matrix.Identity;

            m.M11 = x.X; m.M21 = x.Y; m.M31 = x.Z;
            m.M12 = y.X; m.M22 = y.Y; m.M32 = y.Z;
            m.M13 = z.X; m.M23 = z.Y; m.M33 = z.Z;

            return m;
        }

        //
        //  Calculate the angle between tw vectors.
        //	The returned angle is between 0 and PI (in radians).
        //

        public static float Angle(Vector3 v1, Vector3 v2)
        {
            float norm1, norm2, angle;

            norm1 = v1.Length();
            norm2 = v2.Length();

            if (norm1 > 0.0f && norm2 > 0.0f)
                angle = (float)Math.Acos(Vector3.Dot(v1, v2) / (norm1 * norm2));
            else
                angle = -1.0f;

            return angle;
        }

        //
        //	Make a rotation matrix that rotates v1 into v2 direction
        //

        public static Matrix CreateRotate(Vector3 v1, Vector3 v2)
        {
            Matrix m = Matrix.Identity;

            Vector3 p;	// the rotation axis
            float angle;	// the rotation angle

            angle = Angle(v1, v2);
            if (angle > 0.0f)	// v1 and v2 are not in the same direction;
            {
                Vector3.Cross(ref v1, ref v2, out p);
                if (p.Length() > 0.0f)
                {
                    Quaternion q = Quaternion.CreateFromAxisAngle(p, angle);
                    m = Matrix.CreateFromQuaternion(q);
                }
                else			// v1 and v2 are colinear but in opposite direction
                {	// Find a non-zero vertor perpendicular to v1
                    if (v1.X != 0.0f || v1.Y != 0.0f)
                    {
                        p.X = -v1.Y;
                        p.Y = v1.X;
                        p.Z = 0.0f;
                    }
                    else
                    {
                        p.X = 1.0f;
                        p.Y = p.Z = 0.0f;
                    }
                    // Make a rotation matrix (180 degrees around p)
                    Quaternion q = Quaternion.CreateFromAxisAngle(p, (float)Math.PI);
                    m = Matrix.CreateFromQuaternion(q);
                }
            }

            return m;
        }

        //
        //	Make a 4X4 matrix that transforms p1 to q1 and p2 to q2
        //
        public static Matrix CreateLineToLineTransform(Vector3 p1, Vector3 p2,
            Vector3 q1, Vector3 q2)
        {
            Vector3 vp, vq;
            float np, nq, s;
            Matrix m, m1;

            vp = p2 - p1;
            vq = q2 - q1;

            m = CreateRotate(vp, vq);

            np = vp.Length();
            nq = vq.Length();
            if (np > 0.0f)
                s = nq / np;
            else
                s = 1.0f;

            // Compute the required matrix.
            Matrix.CreateTranslation(ref q1, out m1);
            m1.M11 = s;
            m1.M22 = s;
            m1.M33 = s;
            m *= m1;
            p1 = -p1;
            Matrix.CreateTranslation(ref p1, out m1);
            m *= m1;

            return m;
        }

    }
}
